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DETERMINE FLOOR REACTION FORCE VERTICAL COMPONENT 
TRAJECTORY PARAMETERS OF CURRENT TIME GAIT. 
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DETERMINE FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT 
PERMISSIBLE RANGE [Mzmin,Mzmax] OF CURRENT TIME GAIT. 
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FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE LOWER LIMIT VALUE Fxmin 
AND FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE UPPER LIMIT VALUE Fxmax 
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CALCULATE PROVISIONAL CURRENT TIME GAIT UNTIL END TIME ON THE BASIS OF 
PROVISIONAL DESIRED ZMP AND OTHER CURRENT TIME GAIT PARAMETERS. 
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DETERMINE TERMINAL DIVERGENT COMPONENT qO[k] ACCORDING TO THE FOLLOWING 
EQUATION FROM BODY POSFTIONAELOCITY (Xe,Ve) AT END OF CURRENT TIME GAIT. 
qO[k] = Xe + Vxe/cuO 
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DETERMINE TERMINAL DIVERGENT COMPONENT ERROR errq 
ACCORDING TO THE FOLLOWING EQUATION: 
errq = qO[k] - q" 
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CALCULATE PROVISIONAL CURRENT TIME GAIT UNTIL END TIME ON THE BASIS OF DESIRED ZMP 
OBTAINED BY ADDING CORRECTION TO PROVISIONAL DESIRED ZMP ACCORDING 
TO RELATIONSHIP OF FIG. 44, ASSUMING THAT a= A a. 
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DETERMINE TERMINAL DIVERGENT COMPONENT ql[k] ACCORDING TO THE FOLLOWING EQUATION 
ON THE BASIS OF BODY POSmONAELOOTY (Xel,Vxel) AT END OF CURRENT TIME GAIT 
RECALCULATED ON THE BASIS OF DESIRED ZMP TO WHICH CORRECTION HAS BEEN ADDED: 

ql[k] = Xel +Vxel/a;0 



DETERMINE PARAMETER SENSITIVITY r ACCORDING TO THE FOLLOWING EQUATION: 
r = (ql[k]-qO[k])/Aa 
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ADD CORRECTION AMOUNT BASED ON a=-errq/r TO PROVISIONAL 
DESIRED ZMP TO PROVIDE UPDATED PROVISIONAL DESIRED ZMP. 
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DETERMINE BODY INCLINATION RESTORING MOMENT ZMP-CONVERTED VALUE PATTERN ON THE BASIS OF 
DIFFERENCE BETWEEN TERMINAL BODY POSTURE ANGLE OF PROVISIONAL CURRENT TIME GAIT AND INITIAL 
BODY POSTURE ANGLE OF NORMAL GAIT AND DIFFERENCE BETWEEN TERMINAL BODY POSTURE ANGULAR 
VELOCITY OF PROVISIONAL CURRENT TIME GAIT AND INITIAL BODY POSTURE ANGULAR VELOCITY 
OF NORMAL GAIT. 



DETERMINE, AS DESIRED ZMP PATTERN, THE PATTERN OBTAINED BY ADDING BODY INCLINATION 
RESTORING MOMENT ZMP-CONVERTED VALUE PATTERN TO PROVISIONAL DESIRED ZMP PATTERN. 
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DETERMINE ANTIPHASE ARM SWING RESTORING ANGULAR ACCELERATION PATTERN ON THE BASIS 
OF DIFFERENCE BETWEEN TERMINAL ANTIPHASE ARM SWING ANGLE OF PROVISIONAL CURRENT TIME 
GAIT AND INITIAL ANTIPHASE ARM SWING ANGLE OF NORMAL GAIT AND DIFFERENCE BETWEEN TERMINAL 
ANTIPHASE ARM SWING ANGULAR VELOCITY OF PROVISIONAL CURRENT TIME GAIT AND INITIAL 
ANTIPHASE ARM SWING ANGULAR VELOCITY OF NORMAL GAIT. 
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DETERMINE DESIRED FLOOR REACTION FORCE VERTICAL COMPONENT 
AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE DESIRED ZMP AT CURRENT TIME 
ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE DESIRED POSITIONS/POSTURES OF BOTH FEET, REFERENCE BODY POSTURE 
AND REFERENCE ARM POSTURE AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



CALCULATE TOTAL CENTER-0 F-GRAVITY VERTICAL POSITIONAELOCITY 
THAT SATISFIES DESIRED FLOOR REACTION FORCE VERTICAL COMPONENT. 
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CALCULATE BODY VERTICAL POSITION THAT SATISFIES 
TOTAL CENTER-OF-GRAVITY VERTICAL POSITION. 
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DETERMINE FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE 
RANGE [Fxmin, Fxmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



DETERMINE FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE 
RANGE [Mzmin,Mzmax3 AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE HORIZONTAL BODY ACCELERATION AND BODY POSTURE ANGULAR 
ACCELERATION SUCH THAT DEISRED ZMP IS SATISFIED, FLOOR REACTION FORCE 
HORIZONTAL COMPONENT Fx DOES NOT EXCEED [Fxmin,Fxmax], AND BODY POSTURE 
ANGLE TRAJECTORY CONVERGES TO NORMAL GAIT, AND ALSO DETERMINE ANTIPHASE 
ARM SWING ANGULAR ACCELERATION SUCH THAT FLOOR REACTION FORCE MOMENT 
VERTICAL COMPONENT Mz DOES NOT EXCEED [Mzmin,Mzmax] AND ANTIPHASE ARM 
SWING ANGLE TRAJECTORY CONVERGES TO NORMAL GAIT. 



INTEGRATE HORIZONTAL BODY ACCELERATION AND BODY POSTURE ANGULAR 
ACCELERATION TO CALCULATE HORIZONTAL BODY VELOCITY AND BODY POSTURE 
ANGULAR VELOCITY. FURTHER INTEGRATE THE RESULT TO DETERMINE HORIZONTAL 
BODY POSITION AND BODY POSTURE. 
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INTEGRATE ANTIPHASE ARM SWING ACCELERATION TO CALCULATE ANTIPHASE ARM SWING 
ANGULAR VELOCITY. FURTHER INTEGRATE THE RESULT TO DETERMINE 
ANTIPHASE ARM SWING ANGLE. 
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CARRY OUT THE SAME PROCESSING AS PROCESSING (S504 TO S528) 
FOR CALCULATING HORIZONTAL BODY ACCELERATION a, BODY 
ANGULAR ACCELERATION £ , AND ANTIPHASE ARM SWING ANGULAR 
ACCELERATION 13 a IF CURRENT TIME IS NOT IN BODY INCLINATION 
ANGLE/ANTIPHASE ARM SWING ANGLE RESTORING PERIOD. 
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DETERMINE HORIZONTAL BODY ACCELERATION a tmp REQUIRED 
TO SATISFY DESIRED ZMP AT CURRENT TIME (TIME k) IF MOTION 
OF BODY TRANSLATIONAL MODE IS PERFORMED. 



CALCULATE INSTANTANEOUS VALUE ZMPrec OF BODY 
INCLINATION RESTORING MOMENT ZMP-CONVERTED 
VALUE PATTERN AT CURRENT TIME. 
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CALCULATE INSTANTANEOUS VALUE /S arec OF ANTIPHASE ARM SWING 
RESTORING ANGULAR ACCELERATION PATTERN AT CURRENT TIME. 
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DETERMINE FLOOR REACTION FORCE HORIZONTAL COMPONENT Fx 
WHEN HORIZONTAL BODY ACCELERATION IS a. 
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PARAMETERS OF NORMAL GAIT 
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SUBROUTINE FOR DETERMINING INITIAL STATES OF NORMAL GAIT 
(INITIAL DIVERGENT COMPONENT, INITIAL BODY POSITION/ VELOCITY, 
INITIAL BODY POSTURE ANGLE/ANGULAR VELOCITY, 
AND INITIAL ANTIPHASE ARM SWING ANGLE/ANGULAR VELOCITY) 



PROVISIONALLY DETERMINE GAIT PARAMETERS 
OF CURRENT TIME GAIT. 
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SUBROUTINE FOR CORRECTING CURRENT TIME GAIT PARAMETERS 
(CORRECT CURRENT TIME GAIT PARAMETERS SO THAT TERMINAL 
DIVERGENT COMPONENT OF CURRENT TIME GAIT AGREES WITH 
INITIAL DIVERGENT COMPONENT OF NORMAL TURNING GAIT AND 
THAT ANTIPHASE ARM SWING ANGLE CONVERGES TO ANTIPHASE 
ARM SWING ANGLE TRAJECTORY OF NORMAL GAIT.) 



DETERMINE PARAMETERS OF FLOOR REACTION FORCE MOMENT 
HORIZONTAL COMPONENT PERMISSIBLE RANGE FOR COMPLIANCE 
CONTROL AND FLOOR REACTION FORCE MOMENT VERTICAL 
COMPONENT COMPENSATION AMOUNT PERMISSIBLE RANGE. 
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SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF CURRENT TIME GAIT 
(DETERMINE IT SUCH THAT MODEL MANIPULATION FLOOR REACTION FORCE MOMENT 
HORIZONTAL COMPONENT IS GENERATED ABOUT DESIRED ZMP AND THAT FLOOR 
REACTION FORCE MOMENT VERTICAL COMPONENT DOES NOT EXCEED FLOOR 
REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE.) 



CORRECT INSTANTANEOUS VALUE OF CURRENT TIME GAIT. 
(CORRECT INSTANTANEOUS VALUE OF CURRENT TIME GAIT SUCH THAT MODEL 
MANIPULATION FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT IS 
ADDITIONALLY GENERATED ABOUT DESIRED ZMP.) 



S3034 



t = t + At 



S3036 



Title: "CONTROL DEVICE FOR LEGGED MOBILE ROBOT" 
First Named Inventor: Toru Takenaka 
National Stage of PCT/JP2004/009472 
Customer No. 40854; Docket No. SAT-16280 
Page 47 of 74 



47/74 



( ENTRY ) 



FIG.57 



DETERMINE DESIRED FLOOR REACTION FORCE VERTICAL 
COMPONENT AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE DESIRED ZMP AT CURRENT TIME 
ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE DESIRED POSITIONS/POSTURES OF BOTH FEET, REFERENCE BODY POSTURE AND 
REFERENCE ARM POSTURE AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



CALCULATE TOTAL ONTER-OF-GRAVITY VERTICAL POSITION/VELOCITY 
THAT SATISFY DESIRED FLOOR REACTION FORCE VERTICAL COMPONENT. 
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CALCULATE VERTICAL BODY POSITION THAT SATISFIES 
TOTAL CENTER-OF-GRAVITY VERTICAL POSITION. 
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DETERMINE FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE RANGE 
[Fxmin,Fxmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE 
[Mzmin,Mzmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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[Mxymin^lxymax] AND FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT COMPENSATION 
AMOUNT PERMISSIBLE RANGE [Mzani^Mzcmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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ZMP, FLOOR REACTION FORCE HORIZONTAL COMPONENT Fx DOES NOT EXCEED [Fxmin,Fxraax], AND 
BODY POSTURE ANGLE TRAJECTORY CONVERGES TO NORMAL GAIT, AND ALSO DETERMINE ANTIPHASE 
ARM SWING ANGULAR ACCELERATION SUCH THAT FLOOR REACTION FORCE MOMENT VERTICAL 
COMPONENT Mz DOES NOT EXCEED [Mzmin,Mzmax] AND ANTIPHASE ARM SWING ANGLE 
TRAJECTORY CONVERGES TO NORMAL GAIT. 
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INTEGRATE ANTIPHASE ARM SWING ACCELERATION TO CALCULATE ANTIPHASE ARM SWING ANGULAR VELOCITY. 
FURTHER INTEGRATE THE RESULT TO DETERMINE ANTIPHASE ARM SWING ANGLE. 
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DETERMINE HORIZONTAL BODY ACCELERATION o tmp REQUIRED TO GENERATE MODEL 
MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT ABOUT 
DESIRED ZMP AT CURRENT TIME (TIME k) IF IT IS ASSUMED THAT MOTION OF BODY 
TRANSLAT10NAL MODE IS PERFORMED 
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AND BODY ANGULAR ACCELERATION 0 OF BODY ROTATION MODE ACCORDING TO 

THE FOLLOWING EQUATIONS: 
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ASSUMED THAT MOTION OF HORIZONTAL BODY ACCELERATION OF BODY TRANSLATIONAL 
MODE DENOTED AS a , BODY ANGULAR ACCELERATION OF BODY ROTATION MODE 
DENOTED 0 , AND ANTIPHASE ARM SWING ANGULAR ACCELERATION DENOTED AS 
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MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT ABOUT DESIRED 
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FIG.60 



INITIALIZATION (t=0, ETC) 
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S2020 



READ NEXT TIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
NEXT BUT ONETIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
CURRENT TIME GAIT CYCLE, AND NEXT TIME GAIT CYCLE. 



SUBROUTINE FOR DETERMINING GAIT 
PARAMETERS OF NORMAL GAIT 



S2022 



S2024 



SUBROUTINE FOR DETERMINING INITIAL STATES OF NORMAL GAIT 
(INITIAL DIVERGENT COMPONENT, INITIAL BODY POSITION/ VELOCITY, 
INITIAL BODY POSTURE ANGLE/ANGULAR VELOCITY, 
AND INITIAL ANTIPHASE ARM SWING ANGLE/ANGULAR VELOCITY) 



PROVISIONALLY DETERMINE GAIT PARAMETERS OF CURRENT TIME GAIT. 



S2026 



SUBROUTINE FOR CORRECTING CURRENT TIME GAIT PARAMETERS 
(CORRECT CURRENT TIME GAIT PARAMETERS SO THAT TERMINAL 
DIVERGENT COMPONENT OF CURRENT TIME GAIT AGREES WITH 
INITIAL DIVERGENT COMPONENT OF NORMAL TURNING GAIT AND 
THAT ANTIPHASE ARM SWING ANGLE CONVERGES TO ANTIPHASE 
ARM SWING ANGLE TRAJECTORY OF NORMAL GAIT.) 



S2028 



S2030 



DETERMINE PARAMETERS OF FLOOR REACTION FORCE MOMENT HORIZONTAL 
COMPONENT PERMISSIBLE RANGE FOR COMPLIANCE CONTROL 



SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF ORIGINAL GAIT 
(DETERMINE INSTANTANEOUS VALUE OF ORIGINAL GAIT SUCH THAT FLOOR REACTION 
FORCE MOMENT HORIZONTAL COMPONENT ABOUT DESIRED ZMP IS ZERO.) 



S2032 



S2034 



SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF CORRECTED GAIT 
(DETERMINE INSTANTANEOUS VALUE OF CORRECTED GAIT SUCH THAT MODEL MANIPULATION 
FLOOR REACTION FORCE MOMENT (HORIZONTAL COMPONENT AND VERTICAL COMPONENT) IS 
ADDITIONALLY GENERATED ABOUT CORRECTED DESIRED ZMP WHILE CORRECTING DESIRED ZMP 
AND ANTIPHASE ARM SWING RESTORING ANGULAR ACCELERATION PATTERN TO APPROXIMATE 
TO ORIGINAL GAIT AT THE SAME TIME.) 
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FIG.61 



DETERMINE DESIRED FLOOR REACTION FORCE VERTICAL 
COMPONENT AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S2T00 



DETERMINE DESIRED ZMP AT CURRENT TIME ON 
THE BASIS OF GAIT PARAMETERS. 



S2T02 



DETERMINE DESIRED POSITIONS/POSTURES OF BOTH FEET, REFERENCE BODY POSTURE 
AND REFERENCE ARM POSTURE AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S2T04 



CALCULATE TOTAL CENTER-OF-GRAVITY VERTICAL POSITION/VELOCITY 
THAT SATISFY DESIRED FLOOR REACTION FORCE VERTICAL COMPONENT. 



S2106 



CALCULATE VERTICAL BODY POSITION THAT SATISFIES 
TOTAL CENTER-OF-GRAVITY VERTICAL POSITION. 



S2108 



DETERMINE FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE 
RANGE [Fxmin,Fxmox] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE 
RANGE [Mzmin,Mzmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S21T1 



DETERMINE FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT PERMISSIBLE 
RANGE [Mxymin,Mxymax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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S2114 



DETERMINE MODEL MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT, 
DESIRED FLOOR REACTION FORCE MOMENT (HORIZONTAL COMPONENT AND VERTICAL COMPONENT) 
FOR COMPLIANCE CONTROL, BODY HORIZONTAL ACCELERATION, BODY POSTURE INCLINATION 
ANGULAR ACCELERATION, AND ANTIPHASE ARM SWING ANGULAR ACCELERATION SUCH THAT 
CONDITIONS OF FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT PERMISSIBLE 
RANGE, FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE, 
AND FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE RANGE ARE SATISFIED. 



S2 



INTEGRATE HORIZONTAL BODY ACCELERATION AND BODY POSTURE ANGULAR ACCELERATION TO 

CALCULATE HORIZONTAL BODY VELOCITY AND BODY POSTURE ANGULAR VELOCITY. 

FURTHER INTEGRATE THE RESULT TO DETERMINE HORIZONTAL BODY POSITION AND BODY POSTURE. 



16 



S2118 



INTEGRATE ANTIPHASE ARM SWING ACCELERATION TO CALCULATE ANTIPHASE ARM SWING ANGULAR 
VELOCITY. FURTHER INTEGRATE THE RESULT TO DETERMINE ANTIPHASE ARM SWING ANGLE. 
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FIG.62 
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DETERMINE DIFFERENCE IN HORIZONTAL BODY POSITION BETWEEN MODELS, 
WHICH IS THE DIFFERENCE BETWEEN HORIZONTAL BODY POSITION OF 
CORRECTED GAIT AND HORIZONTAL BODY POSITION OF ORIGINAL GAIT. 
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DETERMINE DIFFERENCE IN BODY POSTURE INCLINATION ANGLE BETWEEN MODELS, 
WHICH IS THE DIFFERENCE BETWEEN BODY POSTURE INCLINATION ANGLE OF CORRECTED 
GAIT AND BODY POSTURE INCLINATION ANGLE OF ORIGINAL GAIT. 
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DETERMINE DIFFERENCE IN ANTIPHASE ARM SWING ANGLE BETWEEN MODELS, 
WHICH IS THE DIFFERENCE BETWEEN ANTIPHASE ARM SWING ANGLE OF 
CORRECTED GAIT AND ANTIPHASE ARM SWING ANGLE OF ORIGINAL GAIT. 
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DETERMINE REQUIRED VALUE OF MODEL HORIZONTAL BODY POSITION STABILIZATION FLOOR 
REACTION FORCE MOMENT NECESSARY TO CONVERGE DIFFERENCE TO ZERO ON THE BASIS OF 
DIFFERENCE IN HORIZONTAL BODY POSITION BETWEEN MODELS. 
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DETERMINE REQUIRED VALUE OF MODEL BODY POSTURE INCLINATION ANGLE STABILIZATION 
FLOOR REACTION FORCE MOMENT NECESSARY TO CONVERGE DIFFERENCE TO ZERO ON THE BASIS 
OF DIFFERENCE IN BODY POSTURE INCLINATION ANGLE BETWEEN MODELS. 



DETERMINE REQUIRED VALUE OF MODEL ANTIPHASE ARM SWING ANGLE STABILIZATION FLOOR 
REACTION FORCE MOMENT NECESSARY TO CONVERGE DIFFERENCE TO ZERO ON THE BASIS OF 
DIFFERENCE IN ANTIPHASE ARM SWING ANGLE BETWEEN MODELS. 



S2208 

S2210 
S22T2 



DETERMINE MODEL HORIZONTAL BODY POSITION STABILIZATION MOMENT, MODEL BODY POSTURE ANGLE 
STABILIZATION MOMENT, MODEL ANTIPHASE ARM SWING ANGLE STABILIZATION MOMENT, HORIZONTAL 
BODY ACCELERATION, BODY POSTURE ANGULAR VELOCITY, AND ANTIPHASE ARM SWING ANGULAR 
ACCELERATION SUCH THAT THEY SATISFY RESTORING CONDITIONS. 



MODEL MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT 
= MODEL HORIZONTAL BODY POSITION STABILIZATION MOMENT 
+ MODEL BODY POSTURE ANGLE STABILIZATION MOMENT 



S22T4 



DESIRED FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT FOR COMPUANCE CONTROL 
= COMPENSATING TOTAL FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT Mdmdxy 
+ MODEL MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT 



S22T6 



DESIRED FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT FOR COMPUANCE CONTROL 
= COMPENSATING TOTAL FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT Mdmdz 
+ FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT BALANCING WITH CORRECTED GAIT 



S2218 
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INITIALIZATION (t=0, ETC.) 
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WAIT FOR TIMER INTERRUPT (FOR EACH CONTROL CYCLE). | $3514 



S3516 yes 



IS GAIT 
CHANGING? 
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t = 0 S3518 



S3520 



READ NEXT TIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
NEXT BUT ONE TIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
CURRENT TIME GAIT CYCLE, AND NEXT TIME GAIT CYCLE. 



SUBROUTINE FOR DETERMINING GAIT 
PARAMETERS OF NORMAL GAIT 



S3522 



S3524 



SUBROUTINE FOR DETERMINING INITIAL STATES OF NORMAL GAIT 
(INITIAL DIVERGENT COMPONENT, INITIAL BODY POSITION/ VELOCITY, 
INITIAL BODY POSTURE ANGLE/ANGULAR VELOCITY, AND INITIAL 
ANTIPHASE ARM SWING ANGLE/ANGULAR VELOCITY) 



PROVISIONALLY DETERMINE GAIT PARAMETERS 
OF CURRENT TIME GAIT. 



S3526 



SUBROUTINE FOR CORRECTING CURRENT TIME GAIT PARAMETERS 
(CORRECT CURRENT TIME GAIT PARAMETERS SO THAT TERMINAL 
DIVERGENT COMPONENT OF CURRENT TIME GAIT AGREES WITH 
INITIAL DIVERGENT COMPONENT OF NORMAL TURNING GAIT AND 
THAT ANTIPHASE ARM SWING ANGLE CONVERGES TO ANTIPHASE 
ARM SWING ANGLE TRAJECTORY OF NORMAL GAIT.) 



S3528 



S3530 



DETERMINE PARAMETERS OF FLOOR REACTION FORCE HORIZONTAL COMPONENT 
PERMISSIBLE RANGE, FLOOR REACTION FORCE MOMENT HORIZONTAL 
COMPONENT PERMISSIBLE RANGE, AND FLOOR REACTION FORCE MOMENT 
VERTICAL COMPONENT PERMISSIBLE RANGE FOR FULL-MODEL CORRECTION. 



S3532 



SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF CURRENT TIME GAIT 
(DETERMINE IT SUCH THAT DESIRED ZMP IS SATISFIED, FLOOR REACTION FORCE MOMENT 
HORIZONTAL COMPONENT DOES NOT EXCEED aOOR REACTION FORCE MOMENT HORIZONTAL 
COMPONENT PERMISSIBLE RANGE, AND FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT 
DOES NOT EXCEED FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE.) 



DETERMINE INSTANTANEOUS VALUES OF ZMP PERMISSIBLE RANGE, FLOOR REACTION 
FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE, AND FLOOR REACTION 
FORCE HORIZONTAL COMPONENT PERMISSIBLE RANGE FOR FULL-MODEL CORRECTION. 



GENERATE CORRECTED GAIT USING FULL MODEL | $3536 
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FIG.77 



INITIALIZATION (t=0, ETC) 
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WAIT FOR TIMER INTERRUPT (FOR EACH CONTROL CYCLE). I $2314 



S2316 yes 
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CHANGING? 
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1 = 0 | $201 8 



S2320 



READ NEXT TIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
NEXT BUT ONE TIME GAIT'S SUPPORTING LEG COORDINATE SYSTEM, 
CURRENT TIME GAIT CYCLE, AND NEXT TIME GAIT CYCLE 



SUBROUTINE FOR DETERMINING GAIT 
PARAMETERS OF NORMAL GAIT 
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SUBROUTINE FOR DETERMINING INITIAL STATES OF NORMAL GAIT 
(INITIAL DIVERGENT COMPONENT, INITIAL BODY POSITION/ VELOCITY, 
INITIAL BODY POSTURE ANGLE/ANGULAR VaOCITY, AND INITIAL 
ANTIPHASE ARM SWING ANGLE/ANGULAR VELOCITY) 



PROVISIONALLY DETERMINE GAIT PARAMETERS 
OF CURRENT TIME GAIT. 
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SUBROUTINE FOR CORRECTING CURRENT TIME GAIT PARAMETERS 
(CORRECT CURRENT TIME GAIT PARAMETERS SO THAT TERMINAL 
DIVERGENT COMPONENT OF CURRENT TIME GAIT AGREES WITH 
INITIAL DIVERGENT COMPONENT OF NORMAL TURNING GAIT AND 
THAT ANTIPHASE ARM SWING ANGLE CONVERGES TO ANTIPHASE 
ARM SWING ANGLE TRAJECTORY OF NORMAL GAIT.) 



S2328 



S2330 



DETERMINE PARAMETERS OF FLOOR REACTION FORCE MOMENT HORIZONTAL 
COMPONENT PERMISSIBLE RANGE FOR COMPLIANCE CONTROL 



SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF ORIGINAL GAIT 
(DETERMINE INSTANTANEOUS VALUE OF ORIGINAL GAIT SUCH THAT FLOOR 
REACTION FORCE MOMENT HORIZONTAL COMPONENT ABOUT DESIRED ZMP IS 0.) 



S2332 



S2334 



SUBROUTINE FOR DETERMINING INSTANTANEOUS VALUE OF CORRECTED GAIT 

(DETERMINE INSTANTANEOUS VALUE OF CORRECTED GAIT SUCH THAT MODEL MANIPULATION 

aOOR REACTION FORCE MOMENT (HORIZONTAL COMPONENT AND VERTICAL COMPONENT) IS 

ADDITIONALLY GENERATED ABOUT CORRECTED DESIRED ZMP, WHILE CORRECTING DESIRED ZMP 

AND ANTIPHASE ARM SWING RESTORING ANGULAR ACCELERATION PATTERN SO AS TO 

APPROXIMATE TO ORIGINAL GAIT AT THE SAME TIME. HOWEVER, FLOOR REACTION 

FORCE PERMISSIBLE RANGE IS CHANGED ACCORDING TO RESULT OF SLIPPAGE DETERMINATION.) 
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DETERMINE DESIRED FLOOR REACTION FORCE VERTICAL 
COMPONENT AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S5100 



DETERMINE DESIRED ZMP AT CURRENT TIME ON 
THE BASIS OF GAIT PARAMETERS. 
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DETERMINE DESIRED POSITIONS/POSTURES OF BOTH FEET, REFERENCE BODY POSTURE 
AND REFERENCE ARM POSTURE AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S5104 



CALCULATE TOTAL CENTER-OF-GRAVITY VERTICAL POSITIONAELOCITY 
THAT SATISFY DESIRED FLOOR REACTION FORCE VERTICAL COMPONENT. 
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CALCULATE VERTICAL BODY POSITION THAT SATISFIES 
TOTAL CENTER-OF-GRAVITY VERTICAL POSITION. 



S5108 



DETERMINE FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE 
RANGE [Fxmin,Fxmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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DETERMINE FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE 
RANGE [Mzmin,Mzmax] AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 



S5112 



DETERMINE FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT PERMISSIBLE 
RANGE rMxvmin.Mxvmax1 AT CURRENT TIME ON THE BASIS OF GAIT PARAMETERS. 
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yes 
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RESULT OF SLIPPAGE 
DETERMINATION 
= IS THERE SUPPAGE?\no 



GRADUALLY APPROXIMATE PERMISSIBLE RANGE REDUCING RATE att TO oT] 



S5120 



GRADUALLY APPROXIMATE PERMISSIBLE RANGE REDUCING RATE att TO 1 . 1 



MULTIPLY Fxmin, Fxmax, Mzmin, AND Mzmax BY REDUCING RATE att SO AS TO NARROW 
FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE RANGE AND FLOOR 
REACTION FORCE MOMENT HORIZONTAL COMPONENT PERMISSIBLE RANGE. 
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S5124 



DETERMINE MODEL MANIPULATION FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT, 
DESIRED FLOOR REACTION FORCE MOMENT (HORIZONTAL COMPONENT AND VERTICAL COMPONENT) 
FOR COMPLIANCE CONTROL, BODY HORIZONTAL ACCELERATION, BODY POSTURE INCLINATION 
ANGULAR ACCELERATION, AND ANTIPHASE ARM SWING ANGULAR ACCELERATION SUCH THAT 
CONDITIONS OF FLOOR REACTION FORCE MOMENT HORIZONTAL COMPONENT PERMISSIBLE 
RANGE, FLOOR REACTION FORCE MOMENT VERTICAL COMPONENT PERMISSIBLE RANGE, 
AND FLOOR REACTION FORCE HORIZONTAL COMPONENT PERMISSIBLE RANGE ARE SATISFIED. 



INTEGRATE HORIZONTAL BODY ACCELERATION AND BODY POSTURE ANGULAR ACCELERATION TO 

CALCULATE HORIZONTAL BODY VELOCITY AND BODY POSTURE ANGULAR VELOCITY. 

FURTHER INTEGRATE THE RESULT TO DETERMINE HORIZONTAL BODY POSITION AND BODY POSTURE. 



INTEGRATE ANTIPHASE ARM SWING ACCELERATION TO CALCULATE HORIZONTAL BODY VELOCITY 
AND ANTIPHASE ARM SWING ANGULAR VELOCITY. FURTHER INTEGRATE THE RESULT TO 
DETERMINE ANTIPHASE ARM SWING ANGLE. 
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SPIN DETERMINING SUBROUTINE 
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SLIPPAGE VIBRATION DETERMINING SUBROUTINE 
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SLIPPAGE DETERMINATION RESULT = NO SLIPPAGE ! 
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DETERMINE GROUND ANGULAR VELOCITY VERTICAL COMPONENT <osupz OF 
FOOT OF THE SUPPORTING LEG ON THE BASIS OF ACTUAL BODY POSTURE 
ANGULAR VELOCITY AND JOINT ANGLE COMMAND (DETECTION VALUE). 



S5310 



DETERMINE CHANGING RATE dMsupactz/dt OF SUPPORTING LEG FLOOR 
REACTION FORCE MOMENT VERTICAL COMPONENT. 
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SPIN DETERMINATION RESULT = SPIN 
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SPIN DETERMINATION RESULT = NO SPIN 



SPIN DETERMINATION RESULT = NO SPIN 
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DETERMINE GROUND TRANSITIONAL VELOCITY HORIZONTAL COMPONENT Vsupxy OF 
FOOT OF THE SUPPORTING LEG ON THE BASIS OF ACTUAL BODY POSTURE ANGULAR VELOCITY, 
DETECTION VALUE OF ACCELERATION, AND JOINT ANGLE COMMAND (DETECTION VALUE). 



DETERMINE CHANGING RATE dFsupactxy/dt OF SUPPORTING LEG 
FLOOR REACTION FORCE HORIZONTAL COMPONENT. 
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CONSTANT Ksups OF SUPPORTING 
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TRANSLATIONAL SLIPPAGE DETERMINATION RESULT 
= TRANSLATIONAL SLIPPAGE 
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TRANSLATIONAL SUPPAGE DETERMINATION RESULT 
= NO TRANSLATIONAL SLIPPAGE 
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